#ifndef FOURMOTORDRIVESUBSYSTEM_H
#define FOURMOTORDRIVESUBSYSTEM_H

// Include files.
#include <map>
#include "WPILib.h"
#include "DriveSubsystem.hpp"

/**
 *  The FourMotorTankDriveSubsystem class is a base class for all four motor drive subsystems.
 */
class FourMotorTankDriveSubsystem :
	public DriveSubsystem
{
		friend class MecanumDriveSubsystem;
	public:
		/**
		 *  The MotorPosition enum can be used to select drive motors.
		 */
		enum MotorPosition
		{
			MotorPositionLeftFront = RobotDrive::kFrontLeftMotor,
			MotorPositionLeftBack = RobotDrive::kRearLeftMotor,
			MotorPositionRightFront = RobotDrive::kFrontRightMotor,
			MotorPositionRightBack = RobotDrive::kRearRightMotor,
		};
		
		FourMotorTankDriveSubsystem
		(
			UINT32 leftFrontMotorChannel, 
			UINT32 leftBackMotorChannel,
			UINT32 rightFrontMotorChannel, 
			UINT32 rightBackMotorChannel
		);
		
		FourMotorTankDriveSubsystem
		(
			CANJaguar *leftFrontMotor, 
			CANJaguar *leftBackMotor,
			CANJaguar *rightFrontMotor, 
			CANJaguar *rightBackMotor
		);
		
		FourMotorTankDriveSubsystem
		(
			CANJaguar &leftFrontMotor, 
			CANJaguar &leftBackMotor,
			CANJaguar &rightFrontMotor, 
			CANJaguar &rightBackMotor
		);
		
		FourMotorTankDriveSubsystem
		(
			SpeedController *leftFrontMotor, 
			SpeedController *leftBackMotor,
			SpeedController *rightFrontMotor, 
			SpeedController *rightBackMotor
		);
		
		FourMotorTankDriveSubsystem
		(
			SpeedController &leftFrontMotor, 
			SpeedController &leftBackMotor,
			SpeedController &rightFrontMotor, 
			SpeedController &rightBackMotor
		);
		
		virtual ~FourMotorTankDriveSubsystem();
		
		void DriveByTank(double leftValue, double rightValue);
		void DriveByMagnitudeAndCurve(double magnitude, double curve);
		
		// TODO: Add drive distance functions.
		
		void SetControlMode(ControlMode controlMode);
		ControlMode GetControlMode();
		void SetSpeedReference(SpeedReference speedReference);
		void SetDistanceReference(DistanceReference distanceReference);
		void SetNeutralMode(NeutralMode neutralMode);
		
		bool GetMotorInvertation(MotorPosition motorPosition);
		
		void SetMotorInvertations
		(
			bool leftFrontIsInverted, 
			bool leftBackIsInverted, 
			bool rightFrontIsInverted, 
			bool rightBackIsInverted
		);
		
		void SetMotorInvertation(MotorPosition motorPosition, bool isInverted);
		
		void SetEncodersCodesPerRevolution(UINT16 codesPerRevolution);
		
		void SetEncodersCodesPerRevolution
		(
			UINT16 leftFrontCodesPerRevolution, 
			UINT16 leftBackCodesPerRevolution, 
			UINT16 rightFrontCodesPerRevolution, 
			UINT16 rightBackCodesPerRevolution
		);
		
		void SetEncoderCodesPerRevolution
		(
			MotorPosition motorPosition, 
			UINT16 codesPerRevolution
		);
		
		void SetPIDs(ControlMode controlMode, double p, double i, double d);
		
		void SetPID
		(
			ControlMode controlMode,
			MotorPosition motorPosition,
			double p,
			double i,
			double d
		);
		
		void SetMaximumOutput(double maximumOutput);
		
		double GetDistanceThreshold();
		void SetDistanceThreshold(double distanceThreshold);
		
		double GetExpiration();
		void SetExpiration(double expiration);
		
		virtual void Stop();
	protected:
		static CANJaguar &s_nullCANJaguar;
		
		static double GetMotorInvertationMultiplier(bool isInverted);
		
		bool m_driveUpdated;
		bool m_reversed;
		bool m_ownsDrive;
		double m_maxOutput;
		
		bool m_motorInvertationLeftFront; 
		bool m_motorInvertationLeftBack;
		bool m_motorInvertationRightFront;
		bool m_motorInvertationRightBack;
		bool m_usesCanJaguars;
		CANJaguar &m_cjLeftFront;
		CANJaguar &m_cjLeftBack;
		CANJaguar &m_cjRightFront; 
		CANJaguar &m_cjRightBack;
		
		ControlMode m_controlMode;
		int m_counterPeriods;
		
		MotorPosition m_motorCanidateForCheck;
		
		MotorSafetyHelper *m_safetyHelper;
		
		void PrintFaults();
		
		// These are functions inherited from RobotSubsystem.
		virtual void Enable();
		virtual void Periodic();
		virtual void Disable();
	private:
		struct PidValue
		{
			public:
				PidValue();
				PidValue(double p, double i, double d);
				
				double m_p;
				double m_i;
				double m_d;
		};
		
		std::map<ControlMode, std::map<MotorPosition, PidValue> > m_pidValues;
		
		RobotDrive &m_drive;
		double m_distanceThreshold;
		void Startup();
};

#endif // #ifndef FOURMOTORDRIVESUBSYSTEM_H 
